#include "surface_point.hh"
#include "cone_geometry.hh"
#include <math.h>
#include <iostream>
#include <cmath>

using namespace cgmath;
using namespace std;


//Constructor
cone_geometry::cone_geometry (double r, double h)
{
  radius = r;
  height = h;

}

double cone_geometry::hit (const cgmath::ray_3d& r) const
{
  double t1;
  double t2;
  double zmin = 0.0;
  double c0 = (radius / height);
  double c2 = pow(c0, 2.0);

  double a = pow(r.d(0), 2) + pow(r.d(1), 2) - c2*pow(r.d(2), 2);
  double b = 2*r.p(0)*r.d(0) + 2*r.p(1)*r.d(1) - c2*2*r.p(2)*r.d(2);
  double c = pow(r.p(0), 2) + pow(r.p(1), 2) - c2*pow(r.p(2), 2);

  t1 = (-b + sqrt(pow(b,2) - 4*a*c)) / (2*a);
  t2 =  (-b - sqrt(pow(b,2) - 4*a*c)) / (2*a);

  cgmath::vector_3d x1 = r(t1);
  cgmath::vector_3d x2 = r(t2);
  double x11 = r.p(2) + t1*r.d(2);
  double x22 = r.p(2) + t1*r.d(2);

  //cout << "x11: " << x11 << " x22: " << x22 << "\n";

  if(((x11 > zmin) && (x11 < height)) || ((x22 > zmin) && (x22 < height)))
    {
      if(t1 > 0 ||  t2 > 0)
	{
	  if(t1 < t2)
	    {
	      //cout << "t1 closer\n";
	      hit_point = r(t1);
	      hit_normal = vec(2*hit_point(0), 2*hit_point(1), 2*c2*hit_point(2));
	      return t1;
	    } 
	  else 
	  {
	    //cout << "t2 closer\n";
	    hit_point = r(t2);
	    hit_normal = vec(2*hit_point(0), 2*hit_point(1), 2*c2*hit_point(2));
	    return t2;
	  }
	}
    }
    
  return std::numeric_limits<double>::infinity();
    

  /*
  if (cgmath::nearest_point_on (r, s, t))
    {
      hit_point = r(t);
      return t;
    }
  else
    return std::numeric_limits<double>::infinity();
  */
}
  
void cone_geometry::hit_surface_point (surface_point& pt) const 
{ 
  pt.point  = hit_point;
  pt.normal = normalized(hit_normal);
  pt.u = 0.0;
  pt.v = 0.0;
}
